#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>

#ifndef GRIPPER_H
#define GRIPPER_H

typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;

class Gripper{
private:
    GripperClient* gripper_client_;

public:
    //Action client initialization
    Gripper(){

        //Initialize the client for the Action interface to the gripper controller
        //and tell the action client that we want to spin a thread by default
        gripper_client_ = new GripperClient("r_gripper_controller/gripper_action", true);

        //wait for the gripper action server to come up
        while(!gripper_client_->waitForServer(ros::Duration(5.0))){
            ROS_INFO("Waiting for the r_gripper_controller/gripper_action action server to come up");
        }
    }

    ~Gripper(){
        delete gripper_client_;
    }

    //Open the gripper
    void open(){
        pr2_controllers_msgs::Pr2GripperCommandGoal open;
        open.command.position = 0.052;
        open.command.max_effort = -1.0;  // Do not limit effort (negative)

        ROS_INFO("Sending open goal");
        gripper_client_->sendGoal(open);
        gripper_client_->waitForResult();
        if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            ROS_INFO("The gripper opened!");
        else
            ROS_INFO("The gripper failed to open.");
    }

    void openFull()
    {
        pr2_controllers_msgs::Pr2GripperCommandGoal open;
        open.command.position = 0.08;
        open.command.max_effort = -1.0;  // Do not limit effort (negative)

        ROS_INFO("Sending open goal");
        gripper_client_->sendGoal(open);
        gripper_client_->waitForResult();
        if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            ROS_INFO("The gripper opened!");
        else
            ROS_INFO("The gripper failed to open.");
    }

    //Close the gripper
    bool close(){
        pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
        squeeze.command.position = 0.0;
        squeeze.command.max_effort = 50.0;  // Close gently

        ROS_INFO("Sending squeeze goal");
        gripper_client_->sendGoal(squeeze);
        gripper_client_->waitForResult();
        if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("The gripper closed!");
            return true;
        }
        else
        {
            ROS_INFO("The gripper failed to close.");
            return false;
        }
    }
};

#endif // GRIPPER_H
